/*
 * usart1_pdca.c
 *
 * Created: 9/18/2011 5:36:00 PM
 *  Author: GrubyGrub
 */ 
#include <asf.h>
#include "pdca.h"
#include "conf_pdca.h"
#include "marg.h"
#include <stdio.h>
#include <math.h>
#include "ekf.h"

static char buffer[256];

struct marg_sensors_t *marg_s;

#if __GNUC__
__attribute__((__interrupt__))
#elif __ICCAVR32__
#pragma handler = AVR32_PDCA_IRQ_GROUP, 0
__interrupt
#endif
 static void USART1_pdca_handler_out(void)
{
	Disable_global_interrupt();
	pdca_disable(USART1_PDCA_CHANNEL_OUT);
	pdca_disable_interrupt_transfer_complete(USART1_PDCA_CHANNEL_OUT);
	Enable_global_interrupt();
}

#if __GNUC__
__attribute__((__interrupt__))
#elif __ICCAVR32__
#pragma handler = AVR32_PDCA_IRQ_GROUP, 0
__interrupt
#endif
 static void USART1_pdca_handler_in(void)
 {
	Disable_global_interrupt();
	pdca_disable(USART1_PDCA_CHANNEL_IN);
	pdca_disable_interrupt_transfer_complete(USART1_PDCA_CHANNEL_IN);
	Enable_global_interrupt();
 }

/*enable the PDCA channel for USART1*/
void init_pdca_USART1()
{
	static const pdca_channel_options_t PDCA_OPTIONS_OUT =
   {
     .addr = (unsigned int) buffer,   // memory address
     .pid = AVR32_PDCA_PID_USART1_TX,       // select peripheral - data are transmit on USART TX line.
     .size = sizeof(buffer),           // transfer counter
     .r_addr = 0, // next memory address
     .r_size = 0,       // next transfer counter
     .transfer_size = PDCA_TRANSFER_SIZE_BYTE,                // select size of the transfer
   };
   
   static const pdca_channel_options_t PDCA_OPTIONS_IN =
   {
     .addr = (unsigned int) inbuffer,   // memory address
     .pid = AVR32_PDCA_PID_USART1_RX,       // select peripheral - data are transmit on USART TX line.
     .size = sizeof(inbuffer),           // transfer counter
     .r_addr = 0, // next memory address
     .r_size = 0,       // next transfer counter
     .transfer_size = PDCA_TRANSFER_SIZE_BYTE,                // select size of the transfer
   };
  
	Disable_global_interrupt();
	
	INTC_register_interrupt(&USART1_pdca_handler_in,USART1_PDCA_IRQ_IN,USART1_PDCA_IRQ_LEVEL_IN);
	INTC_register_interrupt(&USART1_pdca_handler_out,USART1_PDCA_IRQ_OUT,USART1_PDCA_IRQ_LEVEL_OUT);
	
	Enable_global_interrupt();
	
	pdca_enable_interrupt_transfer_complete(USART1_PDCA_CHANNEL_OUT);
	pdca_enable_interrupt_transfer_complete(USART1_PDCA_CHANNEL_IN);
	pdca_init_channel(USART1_PDCA_CHANNEL_OUT, &PDCA_OPTIONS_OUT); // init PDCA channel with options.
	pdca_init_channel(USART1_PDCA_CHANNEL_IN, &PDCA_OPTIONS_IN); // init PDCA channel with options.
	
	pdca_enable(USART1_PDCA_CHANNEL_IN);
}

void USART1_Refresh_Channel()
{
	pdca_reload_channel(USART1_PDCA_CHANNEL_IN,inbuffer,1024);
}

/*place the datastruct so we can access it*/
void USART1_pdca_MARG(struct marg_sensors_t *marg)
{
	marg_s = marg;
}

void PrintEuler()
{
	/*check if channel is free*/
	//if(pdca_get_channel_status(USART1_PDCA_CHANNEL))
	//	return;
	unsigned int buffersize;
		
	buffersize = sprintf(buffer,"%f %f %f %f %f %f\n",marg_s->p,marg_s->r, marg_s->y,marg_s->kp,marg_s->ki,marg_s->kd);

	/*load the buffersize in*/
	pdca_load_channel(USART1_PDCA_CHANNEL_OUT,buffer,buffersize);
	pdca_enable(USART1_PDCA_CHANNEL_OUT);
}

void PrintQuaternions()
{
	/*check if channel is free*/
	//if(pdca_get_channel_status(USART1_PDCA_CHANNEL))
	//	return;
	unsigned int buffersize;
		
	buffersize = sprintf(buffer,"%f %f %f %f\n",marg_s->state_q0,marg_s->state_q1, marg_s->state_q2, marg_s->state_q3);

	/*load the buffersize in*/
	pdca_load_channel(USART1_PDCA_CHANNEL_OUT,buffer,buffersize);
	pdca_enable(USART1_PDCA_CHANNEL_OUT);
}







